#include <thread>
#include <cmath>
#include <audioplay.hpp>

#define AUDIO_DIALOGUE_FILE_NAME "/home/unitree/project/unictrlsockt/wav/outpud.wav"
#define AUDIO_MUSIC_FILE_NAME "/home/unitree/project/unictrlsockt/wav/cache_segment.wav"
#define PLAY_MUSIC_ID   "MUSIC"
#define PLAY_NAME       "output"

Audioplay::Audioplay() : sample_rate(-1),
                         num_channels(0),
                         filestate(false){
    
}

Audioplay::~Audioplay() {
    // Cleanup if necessary
}

void Audioplay::Init() {
    // Initialize the audio client
    aclient_ = std::make_unique<unitree::robot::g1::AudioClient>();
    aclient_->Init();
    aclient_->SetTimeout(10.f);
}

void Audioplay::Play() {
  while(true) {
    {
      std::unique_lock<std::mutex> lock(audio_mutex_);
      audio_cv_.wait((lock));
    }
    switch (playsta.load()) {
      case Play_Dialogue:
        pcm_list.push_back(ReadWave(AUDIO_DIALOGUE_FILE_NAME, &sample_rate, &num_channels, &filestate));
        std::cout << "sample_rate = " << sample_rate
              << " num_channels =  " << std::to_string(num_channels)
              << " filestate =" << filestate << std::endl;
        if (filestate && sample_rate == 16000 && num_channels == 1) {
          std::list<std::vector<uint8_t>> list = std::move(pcm_list);
          for (auto& pcm : pcm_list) {
            int ret = aclient_->PlayStream(
                PLAY_NAME, std::to_string(unitree::common::GetCurrentTimeMillisecond()),
                std::move(pcm_list.front()));
            pcm_list.pop_front();
            if (ret != 0) {
              std::cout << "play failed!" << std::endl;
            }
          }
          
        // aclient_->PlayStop("example");
        } 
        else {
          std::cout << "audio file format error, please check!" << std::endl;
        } 
        break;
      case Play_Music:
        pcm_list.push_back(ReadWave(AUDIO_MUSIC_FILE_NAME, &sample_rate, &num_channels, &filestate));
        std::cout << "sample_rate = " << sample_rate
              << " num_channels =  " << std::to_string(num_channels)
              << " filestate =" << filestate << std::endl;
        if (filestate && sample_rate == 16000 && num_channels == 1) {
          int ret = aclient_->PlayStream(PLAY_NAME, PLAY_MUSIC_ID, std::move(pcm_list.front()));
          pcm_list.pop_front();
        }
        break;
      case Play_Stop:
        
        break;
    }
  }
}

void Audioplay::LedCtl() {
    const float period = 3.5f;
    const float update_interval_seconds  = 0.02f;
    const int MAX_LUX = 190;
    const int MIN_LUX = 15;
    const float omega = 2 * M_PI / period;
    const float gamma = 0.5f;
    float time_total = 0;
    const std::chrono::microseconds update_interval_micros(static_cast<long long>(update_interval_seconds * 1000000));
    auto next_wake_time = std::chrono::steady_clock::now();
    while (true) {
        {
          std::unique_lock<std::mutex> lock(led_mutex_);
          led_cv_.wait(lock);
        }
        if (wakeup_.load()) {
            do {
                next_wake_time += update_interval_micros; // 计算下一次唤醒的目标时间
                float time_current = fmodf(time_total, period);
                float current_normalized_time = time_current / period; // 0 到 1

                float normalized_lux = 0.5f * (std::cos(omega * time_current + static_cast<float>(M_PI)) + 1.0f);
                float gamma_lux = std::pow(normalized_lux, gamma);
                uint8_t lux = static_cast<uint8_t>(MIN_LUX + gamma_lux * (MAX_LUX - MIN_LUX));

                aclient_->LedControl(0, lux, 0);

                time_total += update_interval_seconds ;
                std::this_thread::sleep_until(next_wake_time);
            }
            while (wakeup_.load());
            aclient_->LedControl(255, 255, 255);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

void Audioplay::Start() {
  void_task_futures_.clear();
  void_task_futures_.push_back(std::async(std::launch::async, &Audioplay::Play, this));
  void_task_futures_.push_back(std::async(std::launch::async, &Audioplay::LedCtl, this));
}

std::vector<uint8_t> Audioplay::ReadWaveImpl(std::istream &is, int32_t *sampling_rate, uint8_t *channelCount, bool *is_ok) {
  is.read(reinterpret_cast<char *>(&header_.chunk_id), sizeof(header_.chunk_id));

  //                        F F I R
  if (header_.chunk_id != 0x46464952) {
    printf("Expected chunk_id RIFF. Given: 0x%08x\n", header_.chunk_id);
    *is_ok = false;
    return {};
  }

  is.read(reinterpret_cast<char *>(&header_.chunk_size),
          sizeof(header_.chunk_size));

  is.read(reinterpret_cast<char *>(&header_.format), sizeof(header_.format));

  //                      E V A W
  if (header_.format != 0x45564157) {
    printf("Expected format WAVE. Given: 0x%08x\n", header_.format);
    *is_ok = false;
    return {};
  }

  is.read(reinterpret_cast<char *>(&header_.subchunk1_id),
          sizeof(header_.subchunk1_id));

  is.read(reinterpret_cast<char *>(&header_.subchunk1_size),
          sizeof(header_.subchunk1_size));

  if (header_.subchunk1_id == 0x4b4e554a) {
    // skip junk padding
    is.seekg(header_.subchunk1_size, std::istream::cur);

    is.read(reinterpret_cast<char *>(&header_.subchunk1_id),
            sizeof(header_.subchunk1_id));

    is.read(reinterpret_cast<char *>(&header_.subchunk1_size),
            sizeof(header_.subchunk1_size));
  }

  if (header_.subchunk1_id != 0x20746d66) {
    printf("Expected subchunk1_id 0x20746d66. Given: 0x%08x\n",
           header_.subchunk1_id);
    *is_ok = false;
    return {};
  }

  if (header_.subchunk1_size != 16 &&
      header_.subchunk1_size != 18) {  // 16 for PCM
    printf("Expected subchunk1_size 16. Given: %d\n", header_.subchunk1_size);
    *is_ok = false;
    return {};
  }

  is.read(reinterpret_cast<char *>(&header_.audio_format),
          sizeof(header_.audio_format));

  if (header_.audio_format != 1) {  // 1 for PCM
    printf("Expected audio_format 1. Given: %d\n", header_.audio_format);
    *is_ok = false;
    return {};
  }

  is.read(reinterpret_cast<char *>(&header_.num_channels),
          sizeof(header_.num_channels));

  *channelCount = (int8_t)header_.num_channels;

  is.read(reinterpret_cast<char *>(&header_.sample_rate),
          sizeof(header_.sample_rate));

  is.read(reinterpret_cast<char *>(&header_.byte_rate),
          sizeof(header_.byte_rate));

  is.read(reinterpret_cast<char *>(&header_.block_align),
          sizeof(header_.block_align));

  is.read(reinterpret_cast<char *>(&header_.bits_per_sample),
          sizeof(header_.bits_per_sample));

  if (header_.byte_rate !=
      (header_.sample_rate * header_.num_channels * header_.bits_per_sample / 8)) {
    printf("Incorrect byte rate: %d. Expected: %d", header_.byte_rate,
           (header_.sample_rate * header_.num_channels * header_.bits_per_sample /
            8));
    *is_ok = false;
    return {};
  }

  if (header_.block_align !=
      (header_.num_channels * header_.bits_per_sample / 8)) {
    printf("Incorrect block align: %d. Expected: %d\n", header_.block_align,
           (header_.num_channels * header_.bits_per_sample / 8));
    *is_ok = false;
    return {};
  }

  if (header_.bits_per_sample != 16) {  // we support only 16 bits per sample
    printf("Expected bits_per_sample 16. Given: %d\n", header_.bits_per_sample);
    *is_ok = false;
    return {};
  }

  if (header_.subchunk1_size == 18) {
    int16_t extra_size = -1;
    is.read(reinterpret_cast<char *>(&extra_size), sizeof(int16_t));
    if (extra_size != 0) {
      printf(
          "Extra size should be 0 for wave from NAudio. Current extra size "
          "%d\n",
          extra_size);
      *is_ok = false;
      return {};
    }
  }

  is.read(reinterpret_cast<char *>(&header_.subchunk2_id),
          sizeof(header_.subchunk2_id));

  is.read(reinterpret_cast<char *>(&header_.subchunk2_size),
          sizeof(header_.subchunk2_size));

  header_.SeekToDataChunk(is);
  if (!is) {
    *is_ok = false;
    return {};
  }

  *sampling_rate = header_.sample_rate;

  // header_.subchunk2_size contains the number of bytes in the data.
  // As we assume each sample contains two bytes, so it is divided by 2 here
  std::vector<int16_t> samples(header_.subchunk2_size / 2);

  is.read(reinterpret_cast<char *>(samples.data()), header_.subchunk2_size);
  if (!is) {
    *is_ok = false;
    return {};
  }

  std::vector<uint8_t> ans(samples.size() * 2);
  for (int32_t i = 0; i != static_cast<int32_t>(samples.size()); ++i) {
    ans[i * 2] = samples[i] & 0xFF;
    ans[i * 2 + 1] = (samples[i] >> 8) & 0xFF;
  }

  *is_ok = true;
  return ans;
}

std::vector<uint8_t> Audioplay::ReadWave(const std::string &filename, int32_t *sampling_rate, uint8_t *channelCount, bool *is_ok) {
    std::ifstream is(filename, std::ifstream::binary);
  auto samples = ReadWaveImpl(is, sampling_rate, channelCount, is_ok);
  return samples;
}

int Audioplay::setVolume(uint8_t volume) {
    int ret = 0;
    ret = aclient_->SetVolume(volume);
    return ret;
}

uint8_t Audioplay::getVolume() {
    uint8_t volume;
    aclient_->GetVolume(volume);
    return volume;
}
